get_started-ROS2

入门ROS2

学习视频链接：https://www.bilibili.com/video/BV16B4y1Q7jQ

学习视频链接：https://www.bilibili.com/video/BV1gr4y1Q7j5

官方examples：https://github.com/ros2/examples

官方API文档：https://docs.ros2.org/foxy/api

工作空间和功能包

配置工作空间

安装相关工具

sudo apt install -y python3-pip # 安装pip3
sudo pip3 install rosdepc # 鱼香ROS基于rosdep源码制作了rosdepc 解决了其在国内初始化失败的问题
sudo apt install python3-colcon-ros # ros2优化后的编译器colcon
sudo apt install python3-colcon-common-extensions # 应该是colcon和一些拓展吧

创建工作空间

mkdir -p ~/learning_ros2_ws/src
cd ~/learning_ros2_ws/src

src目录，完成初始化

sudo rosdepc init # 初始化
rosdepc update # 更新

回到工作空间目录，完成依赖安装

cd ..
rosdepc install -i --from-path src --rosdistro foxy -y # 我的ros是foxy版本的

工作空间目录，编译，编译后会产生build、install、log三个文件夹

因为没有代码所以编译没有输出日志信息

colcon build

创建功能包

src目录，创建功能包

cd ~/learning_ros2_ws/src
ros2 pkg create cpp_pkg_name --build-type ament_cmake  # c++
ros2 pkg create python_pkg_name --build-type ament_python  # python

可以通过 --dependencies指定依赖

节点

以简单的循环打印helloWorld的程序，来熟悉节点的创建及使用

Python

创建功能包及代码文件

cd ~/learning_ros2_ws/src
ros2 pkg create --build-type ament_python learning_node_python
cd learning_node_python/learning_node_python
touch helloworld.py

import rclpy 
from rclpy.node import Node 
import time

class HelloWorldNode(Node):
    def __init__(self, name):
        super().__init__(name) 
    def print_loop(self):
        while rclpy.ok():
            self.get_logger().info("Hello World")
            time.sleep(0.5)

def main(args=None):
    rclpy.init(args=args) 

    hello_world_node = HelloWorldNode("python_helloworld") 
    hello_world_node.print_loop() 

    rclpy.shutdown() 

添加setup.py函数入口，指定节点名称 = 包.文件:函数入口

entry_points={
    'console_scripts': [
        'node_helloworld_python = learning_node_python.helloworld:main'
    ],
},

运行

cd ~/learning_ros2_ws/
colcon build
source install/setup.sh
ros2 run learning_node_python node_helloworld_python 

C++

创建功能包及代码文件

cd ~/learning_ros2_ws/src
ros2 pkg create --build-type ament_cmake learning_node_cpp
cd learning_node_cpp/src
touch helloworld.cpp

默认编译类型为ament_cmake，可以省略

#include "rclcpp/rclcpp.hpp"
#include "unistd.h"

class HelloWorldNode : public rclcpp::Node
{
public:
    HelloWorldNode(std::string name): Node(name){};

    void print_loop()
    {
        while(rclcpp::ok())
        {
            RCLCPP_INFO(this->get_logger(), "Hello World");
            usleep(500000);
        }
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv); // 初始化

    // std::shared_ptr<HelloWorldNode> node(new HelloWorldNode("cpp_helloworld")); 
    auto node = std::make_shared<HelloWorldNode>("cpp_helloworld"); // make_shared 效率高且异常安全
    node->print_loop();

    rclcpp::shutdown(); // 关闭

    return 0;
}

CMakeLists.txt中添加

find_package(rclcpp REQUIRED) # 添加依赖

add_executable(node_helloworld_cpp src/helloworld.cpp) # 创建可执行文件 
ament_target_dependencies(node_helloworld_cpp rclcpp)

# 设置编译后文件生成位置
install(TARGETS
  node_helloworld_cpp
  DESTINATION lib/${PROJECT_NAME}
)

运行

cd ~/learning_ros2_ws/
colcon build --packages-select learning_node_cpp
source install/setup.sh
ros2 run learning_node_cpp node_helloworld_cpp

一些理解

关于节点，个人理解可以表示两种东西，有两种含义

①需要运行的节点，就是ros2 run 功能包 节点要指定的节点。

例如上述setup.py中的node_helloworld_python

例如上述CMakeLists.txt中的node_helloworld_cpp

②代码运行中的节点，就是ros2 node list展现的节点

例如上述的python_helloworld和cpp_helloworld

接口

接口其实就是一种规范，可以解决不同语言之间存在差异的问题

一般将接口统一定义在一个功能包内，方便管理

cd ~/learning_ros2_ws/src
ros2 pkg create interfaces

接口功能包的编译类型是ament_cmake

话题

自定义话题接口

话题接口一般统一放在msg文件夹下，话题接口文件为xxx.msg，首字母大写

假设要发布的内容为机器人的信息，包含机器人名以及速度，编写Robot.msg如下

string name
float32 speed

修改CMakeLists.txt，编译msg文件

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/Robot.msg"
  # DEPENDENCIES 都是原始数据类型 不需要依赖
)

修改package.xml，有相关依赖也要相应添加

<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

编译

colcon build --packages-select interfaces

验证

source install/setup.bash
ros2 interface package interfaces

发布者

#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/robot.hpp"

using namespace std::chrono_literals; //命名空间 使得500ms表述合法

class PublisherNode : public rclcpp::Node
{
public:
    rclcpp::Publisher<interfaces::msg::Robot>::SharedPtr publisher;
    rclcpp::TimerBase::SharedPtr timer;

    PublisherNode(std::string name):Node(name)
    {
        publisher = this->create_publisher<interfaces::msg::Robot>("robot_msg_topic",10);
        // timer = this->create_wall_timer(500ms, [this]() -> void { timer_callback(); });
        timer = this->create_wall_timer(500ms, std::bind(&PublisherNode::timer_callback,this));
    }

    void timer_callback()
    {
        interfaces::msg::Robot robot_msg;
        robot_msg.name = "bonbon";
        robot_msg.speed = 1.2345;
        publisher->publish(robot_msg);
    }
};


int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PublisherNode>("publisher_cpp");
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

订阅者

#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/robot.hpp"

using std::placeholders::_1;

class SubscriberNode : public rclcpp::Node
{
public:
    rclcpp::Subscription<interfaces::msg::Robot>::SharedPtr subscriber;

    SubscriberNode(std::string name) : Node(name)
    {
        subscriber = this->create_subscription<interfaces::msg::Robot>(
            "robot_msg_topic",
            10,
            std::bind(&SubscriberNode::subscriber_callback, this, _1));
    }

    void subscriber_callback(interfaces::msg::Robot::SharedPtr robot_msg)
    {
        RCLCPP_INFO(this->get_logger(), "name: %s, speed:%f ", robot_msg->name.c_str(), robot_msg->speed);
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<SubscriberNode>("subscriber_cpp");
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

配置编译运行

CMakeLists.txt中添加

find_package(rclcpp REQUIRED)
find_package(interfaces REQUIRED)

add_executable(topic_publisher_cpp src/publisher.cpp) 
ament_target_dependencies(topic_publisher_cpp rclcpp interfaces)
add_executable(topic_subscriber_cpp src/subscriber.cpp) 
ament_target_dependencies(topic_subscriber_cpp rclcpp interfaces)
install(TARGETS
  topic_publisher_cpp
  topic_subscriber_cpp
  DESTINATION lib/${PROJECT_NAME}
)

package.xml中添加

<depend>rclcpp</depend>
<depend>interfaces</depend>

编译运行

colcon build 
source install/setup.bash
ros2 run learning_topic_cpp topic_publisher_cpp # shell 1
ros2 run learning_topic_cpp topic_subscriber_cpp # shell 2

服务

自定义服务接口

服务接口一般统一放在srv文件夹下，话题接口文件为xxx.srv，首字母大写

假设要服务接受机器人的期望速度$v_x,v_y$，角速度$w$，中心到轮子的距离为常数$l$

然后运动学解算应答三轮所需的速度$v_1,v_2,v_3$

编写KinematicsSolution.srv如下，这里常数写成大写$L$编译会报错

（第一部分表示请求Request，第二部分表示响应Response）

float32 vx
float32 vy
float32 w
float32 l
---
float32 v1
float32 v2
float32 v3

修改CMakeLists.txt和package.xml

由于之前编译msg文件时配置过，只需要对应加上srv文件接口即可

然后通过colcon build即可编译生成对应服务接口文件

客户端

#include "rclcpp/rclcpp.hpp"
#include "interfaces/srv/kinematics_solution.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

class ClientNode : public rclcpp::Node
{
public:
    rclcpp::Client<interfaces::srv::KinematicsSolution>::SharedPtr client;

    ClientNode(std::string name) : Node(name)
    {
        client = this->create_client<interfaces::srv::KinematicsSolution>("kinematics_solution_service");
    }

    auto send_request(float vx, float vy, float w, float l)
    {
        while (!client->wait_for_service(std::chrono::seconds(1)));

        auto request = std::make_shared<interfaces::srv::KinematicsSolution_Request>();

        request->vx = vx;
        request->vy = vy;
        request->w = w;
        request->l = l;

        RCLCPP_INFO(
            this->get_logger(),
            "send vx:%f, vy:%f, w:%f, l:%f",
            request->vx, request->vy, request->w, request->l);

        return client->async_send_request(request, std::bind(&ClientNode::client_callback, this, _1));
    }

    void client_callback(rclcpp::Client<interfaces::srv::KinematicsSolution>::SharedFuture response)
    {
        auto result = response.get();

        RCLCPP_INFO(
            this->get_logger(),
            "result v1:%f, v2:%f, v3:%f",
            result->v1, result->v2, result->v3);
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ClientNode>("client_cpp");

    srand((int)time(NULL));
    auto f = node->send_request((rand() % 10) / 10.0, (rand() % 10) / 10.0, (rand() % 10) / 10.0, 0.2);

    rclcpp::spin_until_future_complete(node, f); //不能用spin 不然接受到响应后退不出
    // while(f.wait_for(10ms) != std::future_status::ready){rclcpp::spin_some(node);} // 也可以自定义 更灵活

    rclcpp::shutdown();

    return 0;
}

服务端

#include "rclcpp/rclcpp.hpp"
#include "interfaces/srv/kinematics_solution.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

class ServerNode : public rclcpp::Node
{
public:
    rclcpp::Service<interfaces::srv::KinematicsSolution>::SharedPtr server;

    ServerNode(std::string name) : Node(name)
    {
        server = this->create_service<interfaces::srv::KinematicsSolution>(
            "kinematics_solution_service",
            std::bind(&ServerNode::server_callback, this, _1, _2));
    }

    void server_callback(
        interfaces::srv::KinematicsSolution::Request::SharedPtr request,
        interfaces::srv::KinematicsSolution::Response::SharedPtr response)
    {
        RCLCPP_INFO(
            this->get_logger(),
            "receive vx:%f, vy:%f, w:%f, l:%f",
            request->vx, request->vy, request->w, request->l);

        float vx = request->vx;
        float vy = request->vy;
        float vw = request->w * request->l;

        response->v1 = vx + vw;
        response->v2 = (-1.0 / 2) * vx + (sqrt(3) / 2) * vy + vw;
        response->v3 = (-1.0 / 2) * vx + (sqrt(3) / 2) * vy + vw;

        RCLCPP_INFO(
            this->get_logger(),
            "result v1:%f, v2:%f, v3:%f",
            response->v1, response->v2, response->v3);
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ServerNode>("server_cpp");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

配置编译运行

CMakeLists.txt中添加

find_package(rclcpp REQUIRED)
find_package(interfaces REQUIRED)

add_executable(service_client_cpp src/client.cpp) 
ament_target_dependencies(service_client_cpp rclcpp interfaces)
add_executable(service_server_cpp src/server.cpp) 
ament_target_dependencies(service_server_cpp rclcpp interfaces)
install(TARGETS
  service_client_cpp
  service_server_cpp
  DESTINATION lib/${PROJECT_NAME}
)

package.xml中添加

<depend>rclcpp</depend>
<depend>interfaces</depend>

编译运行

colcon build 
source install/setup.bash
ros2 run learning_service_cpp service_server_cpp # shell 1
ros2 run learning_service_cpp service_client_cpp # shell 2

参数

改用客户端的代码，采用参数的方式读取和写入

#include "rclcpp/rclcpp.hpp"
#include "interfaces/srv/kinematics_solution.hpp"

using std::placeholders::_1;

class ClientParamNode : public rclcpp::Node
{
public:
    rclcpp::Client<interfaces::srv::KinematicsSolution>::SharedPtr client;

    float vx = 1.0;
    float vy = 1.0;
    float w = 0.2;
    const float l = 0.2;

    ClientParamNode(std::string name) : Node(name)
    {
        client = this->create_client<interfaces::srv::KinematicsSolution>("kinematics_solution_service");

        this->declare_parameter<float>("vx", vx);
        this->declare_parameter<float>("vy", vy);
        this->declare_parameter<float>("w", w);
    }

    auto send_request()
    {
        while (!client->wait_for_service(std::chrono::seconds(1)));

        auto request = std::make_shared<interfaces::srv::KinematicsSolution_Request>();

        this->get_parameter("vx", request->vx);
        this->get_parameter("vy", request->vy);
        this->get_parameter("w", request->w);
        request->l = this->l;

        RCLCPP_INFO(
            this->get_logger(),
            "send vx:%f, vy:%f, w:%f, l:%f",
            request->vx, request->vy, request->w, request->l);

        return client->async_send_request(request, std::bind(&ClientParamNode::client_callback, this, _1));
    }

    void client_callback(rclcpp::Client<interfaces::srv::KinematicsSolution>::SharedFuture response)
    {
        auto result = response.get();

        RCLCPP_INFO(
            this->get_logger(),
            "result v1:%f, v2:%f, v3:%f",
            result->v1, result->v2, result->v3);
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ClientParamNode>("client_param_cpp");

    rclcpp::WallRate rate(1); // Hz
    srand((int)time(NULL));

    while (rclcpp::ok())
    {
        rclcpp::Parameter vx("vx", (rand() % 10) / 10.0);
        rclcpp::Parameter vy("vy", (rand() % 10) / 10.0);
        rclcpp::Parameter w("w", (rand() % 10) / 10.0);

        // rclcpp::Parameter param_array[3] = {vx, vy, w};
        // std::vector<rclcpp::Parameter> param_vec(param_array, param_array + 3);
        // node->set_parameters(param_vec);
        node->set_parameter(vx);
        node->set_parameter(vy);
        node->set_parameter(w);

        rclcpp::spin_until_future_complete(node, node->send_request());
        rate.sleep();
    }

    rclcpp::shutdown();

    return 0;
}

动作

动作是一种应用层的通信机制，实质上是基于服务和两个话题实现的

动作也是一个客户端/服务端模型，具体流程如下：

①客户端发送请求Goal请求，服务端响应后回复

服务端有回调函数goal_callback，用于处理请求

客户端有回调函数goal_response_callback，用于处理响应

②服务端接收后发布Feedback话题，客户端接收话题

服务端有回调函数accepted_callback，用于接收后进行处理，包括Feedback话题发布以及执行器执行Goal

客户端有回调函数feedback_callback，用于处理接收到的Feedback话题数据

服务端有回调函数cancel_callback，用于处理动作取消

③服务端执行完后将Result发送给客户端，客户端处理

客户端有回调函数result_callback，用于处理Result

自定义动作接口

动作接口一般统一放在action文件夹下，动作接口文件为xxx.action，首字母大写

假设定义的动作的功能为模拟机器人旋转，中间实时反馈当前角度，编写TurnAround.action如下

（第一部分表示目标Goal，第二部分表示结果Result，第三部分表示反馈Feedback）

int32 goal_angle
---
int32[] angles
---
int32 now_angle

由于之前配置过，只需要在CMakeLists.txt对应加上action文件接口编译即可生成对应服务接口文件

客户端

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/turn_around.hpp"

#define UNUSED(x) ((void)x);

using namespace std::chrono_literals;
using namespace std::placeholders;

class ActionClientNode : public rclcpp::Node
{
public:
    rclcpp_action::Client<interfaces::action::TurnAround>::SharedPtr client;
    bool exit; // 退出标志位

    ActionClientNode(std::string name) : Node(name)
    {
        client = rclcpp_action::create_client<interfaces::action::TurnAround>(this, "turn_around_action");
        exit = false;
    }

    auto send_goal(int angle)
    {
        while (!client->wait_for_action_server(1s)); // 等待服务

        // 配置Goal和他的Option
        auto goal = interfaces::action::TurnAround::Goal();
        goal.goal_angle = angle;
        auto options = rclcpp_action::Client<interfaces::action::TurnAround>::SendGoalOptions();
        options.goal_response_callback = std::bind(&ActionClientNode::goal_response_callback, this, _1);
        options.feedback_callback = std::bind(&ActionClientNode::feedback_callback, this, _1, _2);
        options.result_callback = std::bind(&ActionClientNode::result_callback, this, _1);

        // 发送
        RCLCPP_INFO(this->get_logger(), "Sending goal");
        auto goal_handle_future = client->async_send_goal(goal, options);
    }

    void goal_response_callback(std::shared_future<rclcpp_action::ClientGoalHandle<interfaces::action::TurnAround>::SharedPtr> future)
    {
        auto goal_handle = future.get();
        if (!goal_handle)
        {
            RCLCPP_ERROR(this->get_logger(), "Rejected by server");
            exit = true;
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Accepted by server");
        }
    }

    void feedback_callback(rclcpp_action::ClientGoalHandle<interfaces::action::TurnAround>::SharedPtr goal_handle, const std::shared_ptr<const interfaces::action::TurnAround::Feedback> feedback)
    {
        UNUSED(goal_handle);

        RCLCPP_INFO(this->get_logger(), "Feedback %d", feedback->now_angle);

        // 取消
        // client->async_cancel_all_goals();
    }

    void result_callback(const rclcpp_action::ClientGoalHandle<interfaces::action::TurnAround>::WrappedResult &result)
    {
        // 进入回调函数 默认退出
        exit = true;

        switch (result.code)
        {
            case rclcpp_action::ResultCode::SUCCEEDED:
            {
                RCLCPP_INFO(this->get_logger(), "Succeeded");
                break;
            }
            case rclcpp_action::ResultCode::ABORTED:
            {
                RCLCPP_ERROR(this->get_logger(), "Aborted");
                return;
            }
            case rclcpp_action::ResultCode::CANCELED:
            {
                RCLCPP_ERROR(this->get_logger(), "Canceled");
                return;
            }
            default:
            {
                RCLCPP_ERROR(this->get_logger(), "Unknown result code");
                return;
            }
        }

        // 打印
        std::string result_str;
        for (auto angle : result.result->angles)
        {
            result_str += std::to_string(angle) + " ";
        }
        RCLCPP_INFO(this->get_logger(), "Result show the angles during runtime: %s", result_str.c_str());
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ActionClientNode>("action_client_cpp");

    node->send_goal(10);

    while (!node->exit)
        rclcpp::spin_some(node);
    rclcpp::shutdown();

    return 0;
}

服务端

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/turn_around.hpp"

#define UNUSED(x) ((void)x); //避免编译器warning
using namespace std::placeholders;

class ActionServerNode : public rclcpp::Node
{
public:
    rclcpp_action::Server<interfaces::action::TurnAround>::SharedPtr server;

    ActionServerNode(std::string name) : Node(name)
    {
        server = rclcpp_action::create_server<interfaces::action::TurnAround>(
            this,
            "turn_around_action",
            std::bind(&ActionServerNode::goal_callback, this, _1, _2),
            std::bind(&ActionServerNode::cancel_callback, this, _1),
            std::bind(&ActionServerNode::accepted_callback, this, _1));
    }

    rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID &uuid, std::shared_ptr<const interfaces::action::TurnAround::Goal> goal)
    {
        UNUSED(uuid);

        RCLCPP_INFO(this->get_logger(), "Received goal angle %d", goal->goal_angle);

        if (goal->goal_angle > 360 || goal->goal_angle < 0)
        {
            RCLCPP_INFO(this->get_logger(), "Reject");
            return rclcpp_action::GoalResponse::REJECT;
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "Accept and execute");
            return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
        }
    }

    rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::TurnAround>> goal_handle)
    {
        UNUSED(goal_handle);

        RCLCPP_INFO(this->get_logger(), "Received request to cancel");

        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void accepted_callback(const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::TurnAround>> goal_handle)
    {
        // 开个线程处理，快速返回以避免阻塞执行器
        std::thread{std::bind(&ActionServerNode::execute, this, _1), goal_handle}.detach();
    }

    void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::TurnAround>> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "Executing...");

        auto goal = goal_handle->get_goal();
        auto feedback = std::make_shared<interfaces::action::TurnAround::Feedback>();
        auto result = std::make_shared<interfaces::action::TurnAround::Result>();

        // 模拟执行
        rclcpp::Rate loop_rate(1);
        for (int i = 0; (i <= goal->goal_angle) && rclcpp::ok(); i++)
        {
            // 取消
            if (goal_handle->is_canceling())
            {
                goal_handle->canceled(result);
                RCLCPP_INFO(this->get_logger(), "Canceled");
                return;
            }

            // 处理result
            result->angles.push_back(i);

            // 处理feedback
            RCLCPP_INFO(this->get_logger(), "Publish feedback");
            feedback->now_angle = i;
            goal_handle->publish_feedback(feedback);

            loop_rate.sleep();
        }

        // 执行完毕
        goal_handle->succeed(result);
        RCLCPP_INFO(this->get_logger(), "Succeeded");
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ActionServerNode>("action_server_cpp");
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}

配置编译运行

CMakeLists.txt中添加

find_package(rclcpp REQUIRED)
find_package(interfaces REQUIRED)
find_package(rclcpp_action REQUIRED)

add_executable(action_client_cpp src/action_client.cpp) 
ament_target_dependencies(action_client_cpp rclcpp interfaces rclcpp_action)
add_executable(action_server_cpp src/action_server.cpp) 
ament_target_dependencies(action_server_cpp rclcpp interfaces rclcpp_action)
install(TARGETS
  action_client_cpp
  action_server_cpp
  DESTINATION lib/${PROJECT_NAME}
)

package.xml中添加

<depend>rclcpp</depend>
<depend>interfaces</depend>
<depend>rclcpp_action</depend>

编译运行

colcon build --packages-select learning_action_cpp
source install/setup.bash
ros2 run learning_action_cpp action_server_cpp # shell 1
ros2 run learning_action_cpp action_client_cpp # shell 2

OpenCV

机器人视觉开发离不开OpenCV，学习在ROS2中使用OpenCV，以及OpenCV和官方接口Image的互相转换

安装OpenCV

安装依赖

sudo apt-get install build-essential libgtk2.0-dev libavcodec-dev libavformat-dev libjpeg-dev libswscale-dev libtiff5-dev libgtk2.0-dev pkg-config

下载源码编译安装：https://opencv.org/releases/

配置环境

sudo gedit /etc/ld.so.conf 
# 添加 include /usr/local/lib

sudo ldconfig

sudo gedit /etc/bash.bashrc 
# 添加 PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig
# 添加 export PKG_CONFIG_PATH

source /etc/bash.bashrc

cv_bridge版本问题

ros安装后会自带opencv，而且cv_bridge的版本也是对应的，比如我的就是4.2.0

然后我自己安装了opencv4.5.0，所以使用cv_bridge的时候就会报不匹配warning

不过好像不理睬再编译一遍就通过了，似乎也能正常使用

折腾过另外一种方法，不过没法解决问题，仅记录

就是在工作空间编译另一个cv_bridge，工作空间就使用这个cv_bridge

git clone https://github.com/ros-perception/vision_opencv.git -b ros2
cp -r vision_opencv/cv_bridge/ ~/learning_ros2_ws/src/cv_bridge # 我们只需要cv_bridge

修改CMakeLists编译OpenCV版本后编译

cd learning_ros2_ws
colcon build --packages-select cv_bridge --allow-overriding cv_bridge

Mat 和 Image的转换

准确来讲应该是cv::Mat和 sensor_msgs/msg/Image 的转换，需要用到cv_bridge

Mat 转 ImageMsg

sensor_msgs::msg::Image Mat2ImageMsg(cv::Mat mat)
{
    sensor_msgs::msg::Image::SharedPtr msg_ptr = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", mat).toImageMsg();
    return *msg_ptr;
}

ImageMsg 转 Mat

cv::Mat ImageMsg2Mat(sensor_msgs::msg::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg,  sensor_msgs::image_encodings::TYPE_8UC3);
    return cv_ptr->image;
}

功能包编写

通过一个简单的将图像resize的服务，来学习ROS2中的OpenCV的使用

服务接口文件

uint16 resize_width
uint16 resize_height
sensor_msgs/Image input_image
---
sensor_msgs/Image output_image

图像格式转换

创建功能包learning_image_cpp，在include文件夹里编写image_conver.hpp

#pragma once

#include "rclcpp/rclcpp.hpp"
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"

sensor_msgs::msg::Image Mat2ImageMsg(cv::Mat mat)
{
    sensor_msgs::msg::Image::SharedPtr msg_ptr = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", mat).toImageMsg();
    return *msg_ptr;
}

cv::Mat ImageMsg2Mat(sensor_msgs::msg::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_8UC3);
    return cv_ptr->image;
}

客户端

#include "rclcpp/rclcpp.hpp"
#include "opencv2/opencv.hpp"
#include "interfaces/srv/resize_image.hpp"
#include "learning_image_cpp/image_conver.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

class ImageClientNode : public rclcpp::Node
{
public:
    rclcpp::Client<interfaces::srv::ResizeImage>::SharedPtr client;

    ImageClientNode(std::string name) : Node(name)
    {
        client = this->create_client<interfaces::srv::ResizeImage>("image_resize_service");
    }

    auto send_request(cv::Mat &img, int width, int height)
    {
        while (!client->wait_for_service(std::chrono::seconds(1)));

        auto request = std::make_shared<interfaces::srv::ResizeImage_Request>();
        request->input_image = Mat2ImageMsg(img);
        request->resize_width = width;
        request->resize_height = height;

        RCLCPP_INFO(this->get_logger(), "Send image");

        return client->async_send_request(request, std::bind(&ImageClientNode::client_callback, this, _1));
    }

    void client_callback(rclcpp::Client<interfaces::srv::ResizeImage>::SharedFuture response)
    {
        auto result = response.get();
        cv::Mat result_img = ImageMsg2Mat(result->output_image);
        if (!result_img.empty())
        {
            cv::imshow("", result_img);
            cv::waitKey(2000);
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<ImageClientNode>("image_client_cpp");
    cv::Mat img = cv::imread("src/learning_image_cpp/image/test.jpg");
    auto f = node->send_request(img, 256, 256);

    rclcpp::spin_until_future_complete(node, f);
    rclcpp::shutdown();

    return 0;
}

服务端

#include "rclcpp/rclcpp.hpp"
#include "opencv2/opencv.hpp"
#include "interfaces/srv/resize_image.hpp"
#include "learning_image_cpp/image_conver.hpp"

using std::placeholders::_1;
using std::placeholders::_2;

class ImageServerNode : public rclcpp::Node
{
public:
    rclcpp::Service<interfaces::srv::ResizeImage>::SharedPtr server;

    ImageServerNode(std::string name) : Node(name)
    {
        server = this->create_service<interfaces::srv::ResizeImage>(
            "image_resize_service",
            std::bind(&ImageServerNode::server_callback, this, _1, _2));
    }

    void server_callback(
        interfaces::srv::ResizeImage::Request::SharedPtr request,
        interfaces::srv::ResizeImage::Response::SharedPtr response)
    {
        RCLCPP_INFO(this->get_logger(), "Receive image");

        cv::Mat input_img = ImageMsg2Mat(request->input_image);
        if(input_img.empty())
        {
            response->output_image = request->input_image;
            RCLCPP_INFO(this->get_logger(), "The image is empty");
            return;
        }

        cv::Mat output_img;
        cv::resize(input_img, output_img, cv::Size(request->resize_width, request->resize_height));
        response->output_image = Mat2ImageMsg(output_img);
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ImageServerNode>("image_server_cpp");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

配置编译运行

CMakeLists.txt中添加

find_package(rclcpp REQUIRED)
find_package(interfaces REQUIRED)
find_package(OpenCV REQUIRED)
find_package(cv_bridge REQUIRED)

include_directories(
  include
)

add_executable(service_image_client_cpp src/image_client.cpp) 
ament_target_dependencies(service_image_client_cpp rclcpp interfaces OpenCV cv_bridge)
add_executable(service_image_server_cpp src/image_server.cpp) 
ament_target_dependencies(service_image_server_cpp rclcpp interfaces OpenCV cv_bridge)
install(TARGETS
  service_image_client_cpp
  service_image_server_cpp
  DESTINATION lib/${PROJECT_NAME}
)

package.xml中添加

<depend>rclcpp</depend>
<depend>interfaces</depend>
<depend>OpenCV</depend>
<depend>cv_bridge</depend>

编译运行

colcon build --packages-select learning_image_cpp
source install/setup.bash
ros2 run learning_image_cpp service_image_server_cpp # shell 1
ros2 run learning_image_cpp service_image_client_cpp # shell 2